COMMENT ⊗   VALID 00007 PAGES
C REC  PAGE   DESCRIPTION
C00001 00001
C00002 00002	ENTRY
C00006 00003	!	arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
C00014 00004	!	drivecode,opclcode,jtmove,driveproc
C00017 00005	!	centerproc
C00018 0000ε	!	opening, opclproc
C00019 00007	!	caseproc,onproc
C00020 ENDMK
C⊗;
ENTRY;
BEGIN "PPROC2"
DEFINE $$PRGID=TRUE;	

DEFINE $PPROC2=TRUE;
DEFINE $ALTER_EGO=TRUE;

REQUIRE "HEADER.SAI" SOURCE_FILE;

!	cmonproc;
ifc false thenc
RECURSIVE PROCESURE DURCM;
	BEGIN
	RPTR(EXPR$) EXP;
	GTOKEN;
	IF TOKEN≠">"≠TOKEN≠"≥" THEN ERROR("Need > or ≥ for duration cm"
	EXP←$$GTSCEXPR("=")
endc

PROCEDURE FORCECM(rptr(expr$)e;INTEGER BITOFFSET);
	BEGIN
	INTEGER V; BOOLEAN GE; RPTR(EXPR$)EXP;
	INTEGER I,IPC;
	INTEGER BITS,DEVBITS;
	RPTR(SYMBOL)C;
	DEVBITS←0;
	WORD_READ("(");
	GTOKEN;
	IF EQU(TOKEN,"XHAT") THEN BITS←BITOFFSET
		ELSE IF EQU(TOKEN,"YHAT") THEN BITS←BITOFFSET+'1000
		ELSE IF EQU(TOKEN,"ZHAT") THEN BITS←BITOFFSET+'2000
		ELSE ERROR("FORCECM: only principal directions allowed");
	GTOKEN(")");
	GTOKEN;
	IF TOKEN="≥" OR TOKEN =">" THEN BITS←BITS+'100000
		ELSE IF TOKEN="≤" OR TOKEN="<" THEN BITS←BITS
		ELSE ERROR("FORCECM: need ≥ or < here");
	EXP←$$GTANYEXP("FORCECM",#SC);
	GTOKEN;
	IF EQU(TOKEN,"IN") THEN
		BEGIN
		GTOKEN;
		IF EQU(TOKEN,"HAND") THEN BITS←BITS
			ELSE IF EQU(TOKEN,"STATION") THEN BEGIN BITS←BITS+'400; DEVBITS←DEVBITS+'400; END
			ELSE ERROR("FORCECM: can only specify in HAND or STATION");
		WORD_READ("DO");
		END
	ELSE	BEGIN IF NOT EQU(TOKEN,"DO") THEN ERROR("FORCECM: Need DO here");
		BITS←BITS+'400; ! default is station;
		END;
	WORD_READ("STOP"); BITS←BITS+'10000; ! stop bit;
	GTOKEN;
	IF EQU(TOKEN,"BARM") THEN BEGIN DEVBITS←DEVBITS+'4; BITS←BITS+'4; END
		ELSE IF EQU(TOKEN,"YARM") THEN BEGIN DEVBITS←DEVBITS+1; BITS←BITS+1; END
		ELSE ERROR("FORCECM: can only stop an arm");
	$TMPOFF←$TMPOFF+1;
	$$PCODE←$FRCPCODE(E,EXP,BITS,DEVBITS);
	END;


!	arm motions: movepcode,alongproc,axmovproc, pbyproc,ptoproc
	moveproc, parkingproc;

PROCEDURE MOVEPCODE(RPTR(FRAME) MFRAME;
		 RPTR(EXPR$) ARRAY FDESTS; INTEGER NFDEST);
	BEGIN
	RPTR(SYMBOL) S1,S2; RPTR(FRAME)F1;
	S1←CHECK(FRAME:PNAME[MFRAME],#FR);
	S2←CHECK(FRAME:PNAME[F1←ARM_CHECK(MFRAME)],#FR);
	$$PCODE←$MOVEPCODE(S1,S2,FDESTS,NFDEST);
	END;


INTERNAL PROCEDURE ALONGPROC(STRING AXIS,FRA1);
	BEGIN
	INTEGER I,INDEX;
	RPTR(expr$)SCAL;RPTR(SYMBOL)SYMPTR;RPTR(FRAME)FRAM1;
	INTEGER ARRAY BUFF1[1:3],BUFF3[1:5];
	RPTR(EXPR$)ARRAY PTR[1:3],DEST[1:1];
	$HELP←21;
	SCAL←$$GTANYEXP("distance to be moved along axis",#SC);
	SYMPTR←CHECK(AXIS[1 TO 1]&"HAT",#VT);
	OLDSAV("MOVE"&AXIS[1 TO 1],FRA1);  ! saves for default instructions;
	FRAM1←BELONGS(FRA1,#FR);
	INDEX←0;
 	FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR],
		XSVMUL, XTVADD  DO BUFF3[INDEX←INDEX+1]←I;
	SYMPTR←CHECK(FRA1,#FR);
	INDEX←0;
	IF SYMBOL:INDEX[SYMPTR]>0 THEN
	    FOR I←XAGTVAL, SYMBOL:INDEX[SYMPTR],SYMBOL:OFFSET[SYMPTR]
			DO BUFF1[INDEX←INDEX+1]←I
	ELSE FOR I←XGTVAL, SYMBOL:OFFSET[SYMPTR],XNOOP
			DO BUFF1[INDEX←INDEX+1]←I;
	PTR[1]←αEXPR$(BUFF1,0);
	PTR[2]←SCAL;
	PTR[3]←αEXPR$(BUFF3,0);
	DEST[1]←$AAPPEND(PTR);
	MOVEPCODE(FRAM1,DEST,1);
	END;

	! moves the frame along one axis by a scalar;

INTERNAL PROCEDURE AXMOVPROC;
	BEGIN
	STRING FRA1,AXIS; 
	$HELP←21;
	AXIS←TOKEN[5 TO 5];		
	FRA1←MVFR_READ;	
	WORD_READ("BY");
	ALONGPROC(AXIS,FRA1);
	END;



	! reads/exec TO <fr>+<vt>{wrt <fr>} or BY <vector>{wrt <fr>};

INTERNAL PROCEDURE PBYPROC;
	BEGIN
 	RPTR(FRAME) FRAM1;RPTR(EXPR$)ARRAY FDEST[1:1];
	$HELP←20;
				! MOVE<fr>BY<vt> ≡ MOVE<fr>TO⊗+<vt>;
		TOKEN←OLDOBJ;
		#TOKEN←ID_TYPE;
		STOKEN←TRUE;		
		$CLINR←"+"&$CLINR;
	FDEST[1]←$$GTANYEXP("destination of MOVE",#FR);
	FRAM1←BELONGS (OLDOBJ,#FR);
	MOVEPCODE(FRAM1,FDEST,1);
	END;

INTERNAL PROCEDURE PTOPROC;
	BEGIN
 	RPTR(FRAME) FRAM1; RPTR(EXPR$) ARRAY FDESTS[1:10]; INTEGER NFDEST;
	NFDEST←0;
	$HELP←20;
	DO BEGIN
		FDESTS[NFDEST←NFDEST+1]←$$GTANYEXP("Destination part of MOVE",#FR);
		IF NFDEST=10 THEN ERROR("Pointy cannot currently handle more than a 9 segment move");
		GTOKEN(FALSE);
	   END UNTIL TOKEN≠",";
	STOKEN←TRUE;
	FRAM1←BELONGS (OLDOBJ,#FR);
	MOVEPCODE(FRAM1,FDESTS,NFDEST);
	END;

INTERNAL PROCEDURE MOVEPROC;
	BEGIN
	STRING FR1,AXIS;
	$HELP←20;
	FR1←IDF_READ; 
	GTOKEN;
	OLDSAV("MOVE",FR1);
	IF EQU(TOKEN,"TO") THEN PTOPROC
		ELSE IF EQU(TOKEN,"BY") THEN PBYPROC
	        ELSE ERROR($SYNMSG[9],$SYNMSG[25]);
	GTOKEN(FALSE);
	IF EQU(TOKEN,"ON") THEN ONPROC($$PCODE) ELSE STOKEN←TRUE;
	END;

INTERNAL PROCEDURE PARKINGPROC;
	BEGIN
	STRING PAR; $HELP←25 ;
	GTOKEN(FALSE);
	IF FINAL THEN ASKUSER("MOVE BARM TO BPARK; {MOVE YARM TO YPARK}")
	   ELSE IF EQU(TOKEN,"BARM") THEN ASKUSER("MOVE BARM TO BPARK")
	   ELSE IF EQU(TOKEN,"YARM") THEN ASKUSER("MOVE YARM TO YPARK")
	  ELSE ERROR("can only park BARM or YARM");
	$$PCODE←PARSE;
	END;

!	drivecode,opclcode,jtmove,driveproc;

	! drives the indicated joint of the arm (what): movement is absolute 
	  if how=to, differential if how=by;

PROCEDURE DRIVECODE(STRING WHAT,HOW;INTEGER JOINT;RPTR(EXPR$)SCAL);
	$$PCODE←$DRIVEPCODE((IF EQU(WHAT,"BJT") THEN BLUE
			ELSE YELLOW),HOW,JOINT,SCAL);

	! executes close or open instruction. How determines if the movement is 
	  absolute (to) or differential (by), op indicates the operation(open/close);

INTERNAL PROCEDURE OPCLCODE(STRING OP,HAND,HOW;RPTR(EXPR$)SCAL);
	BEGIN
	IF EQU(HAND,"BHAND")
	   THEN	IF EQU(HOW,"TO") OR EQU(OP,"OPEN")
		   THEN DRIVECODE("BJT",HOW,7,SCAL) 
		   ELSE DRIVECODE("BJT",HOW,7,$APPEND(SCAL,EXPR$1(XSNEG),#SC))
	   ELSE PRINT(#NOTYET);
	END;

	! parses the instruction
		DRIVE BJT|YJT (#) TO|BY <scalar>;

INTERNAL PROCEDURE JTMOVE(STRING WHAT,HOW;INTEGER JOINT);
	BEGIN "J"
	RPTR(EXPR$) SCAL;
	$HELP←22;
  	SCAL←$$GTANYEXP("joint movement angle",#SC);
	OLDSAV("DRIVE",CVS(JOINT)); 			! saves for default instructions;
	IF EQU(WHAT,"BJT") THEN
		DRIVECODE(WHAT,HOW,JOINT,SCAL)
	ELSE PRINT(#NOTYET);
	END "J";

INTERNAL PROCEDURE DRIVEPROC;
	BEGIN
	STRING HOW;
	STRING WHAT;INTEGER JOINT;
	$HELP←22;
	WHAT←IDF_READ;
	IF EQU(WHAT,"BJT") OR EQU(WHAT,"YJT")
	   THEN BEGIN
	 	WORD_READ("(");				! reads "(number)";
		GTOKEN;
		JOINT←INTSCAN(TOKEN,$BRCHR);
		IF JOINT<1 OR JOINT>7
		   THEN ERROR("non existent joint: ",cvs(joint));
		WORD_READ(")");
		HOW←IDF_READ;
		IF EQU(HOW,"BY") OR EQU(HOW,"TO")
		   THEN JTMOVE(WHAT,HOW,JOINT)
		   ELSE BEGIN
			PRINT($SYNMSG[10],$SYNMSG[25]," OR ");
			ERROR($SYNMSG[14],$SYNMSG[25]);
			END;
		END
	   ELSE ERROR("--→ BJT or YJT ",$SYNMSG[25]);
	END;

!	centerproc;

INTERNAL PROCEDURE CENTERPROC;
	BEGIN "PCENTER"
	STRING POS;
	$HELP←24;
	POS←ARM_READ;		! if the arm is not indicated BARM is assumed;
	IF EQU(POS,"BARM")
	   THEN	$$PCODE←$CENTERPCODE(BLUE)
	   ELSE PRINT(#NOTYET);
	END "PCENTER";

!	opening, opclproc;

INTERNAL PROCEDURE OPENING(STRING FIRST,WHAT,HOW);
	BEGIN
	RPTR(EXPR$)SCAL;
	$HELP←23;
	SCAL←$$GTANYEXP("hand opening or closing",#SC);
	OLDSAV(FIRST,WHAT);			! saves for default instructions;
	OPCLCODE(FIRST,WHAT,HOW,SCAL);
	END;

	! parses the instructions
		OPEN <hand> TO|BY <scalar>;
	!	CLOSE <hand> TO|BY <scalar>;

INTERNAL PROCEDURE OPCLPROC(STRING FIRST);
	BEGIN
	STRING WHAT;
	$HELP←23;
	WHAT←HAND_READ;
	GTOKEN;
	IF EQU(TOKEN,"TO") OR EQU(TOKEN,"BY")
	   THEN OPENING(FIRST,WHAT,TOKEN)
	   ELSE ERROR("Need a TO or BY for OPEN/CLOSE statement");
	END;
!	caseproc,onproc;
	
INTERNAL RECURSIVE PROCEDURE CASEPROC;
	BEGIN END;

INTERNAL PROCEDURE ONPROC(RPTR(EXPR$)E(NULL_RECORD));
	BEGIN
	IF $COMPILE=0 THEN ERROR("ON must be inside a procedure");
	$COMPILE←$COMPILE+1;
	GTOKEN;
	IF EQU(TOKEN,"FORCE") THEN FORCECM(E,0)
	  ELSE IF EQU(TOKEN,"TORQUE") THEN FORCECM(E,'3000)
	  ELSE ERROR("ON: only FORCE or TORQUE available");
	$COMPILE←$COMPILE-1;
	END;

END "PPROC2"